/*
 * Part of the program control-test
 * Feed a queue with data from the ADC
 */
#include <xqueue.h>
#include <xtask.h>
#include <rrcon.h>

#include "sensors_read.h"

# define PERIOD 20   // Hz

namespace control_test {
    using namespace kthrobot;

    SensorsReader::SensorsReader()
        : XTask("SensorsReader") {
            SetPeriodHz(PERIOD);
        }

    SensorsReader::~SensorsReader() {
        Join();
    }

    void SensorsReader::TaskCode() {
        while(!MustStop()) {
            ADCRawData raw_data = RRCon::GetADCData();
            data_queue_.Enq(raw_data);
            WaitPeriod();
        }
    }

    int SensorsReader::ConvertToCm(int value, int type) {
        // Convert a raw voltage value to a distance
        // type = 0 for short-range IR sensors
        // type = 1 for long-range IR sensors
        // Return the value in centimers. -1 means an error
        if (type == 0) {
            return ir_calibration_function_(value, 3.5, 16201, 146);
        }
        if (type == 1) {
            return -1; // Unimplemented
        }
        else return -1;
    }

    int SensorsReader::ir_calibration_function_(
            int V, int k, int m_prime, int b_prime) {
        // Calibration formula for the IR sensors. V is the voltage value given
        // by the ADC converter. The other arguments are the parameters.
        return (m_prime / (V + b_prime)) - k;
    }
};
